#include "servo.h"
#include <stdio.h>

//启动定时器，输出PWM方波
void Servo_Init(void)
{
  HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_2);
}

//设置PWM脉宽
void Servo_SetPulse(int width)
{
  htim5.Instance->CCR2 = width;
}

//获取PWM脉宽
int Servo_GetPulse(void)
{
  return htim5.Instance->CCR2;
}

//设置舵机角度
void Servo_SetAngle(int angle)
{
  //软件限位
  if (angle < 0)
  {
    angle = 0;
  }
  
  if (angle > 180)
  {
    angle = 180;
  }
  
  Servo_SetPulse(angle * 2000 / 180 + 500);
}

//获取舵机角度（估计值）
int Servo_GetAngle(void)
{
  return (Servo_GetPulse() - 500) * 180 / 2000;
}

//平滑移动
void Servo_StepMove(int angle, int step, int delay)
{
  //获取初始角度
  int begin = Servo_GetAngle();
  printf("from %d to %d\n", begin, angle);
  
  if (begin < angle)
  {
    for (int i = begin; i <= angle; i+=step)
    {
      Servo_SetAngle(i);
      HAL_Delay(delay);
    }
  }
  
  if (begin > angle)
  {
    for (int i = begin; i >= angle; i-=step)
    {
      Servo_SetAngle(i);
      HAL_Delay(delay);
    }
  }
}
